/**
 * Author: luoqi
 * Created Date: 2025-10-24 23:49:46
 * Last Modified: 2025-11-26 22:46:7
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <dev/xbot.h>
#include "cmds.hpp"

static QShell &cli = QShellObj::obj();
static PowerFlow &joints = XBotObj::obj().joints;
static XBot &dev = XBotObj::obj();

static int args_dbg(int argc, char **argv)
{
    if(argc == 3) {
        if(ISARG(argv[1], "canfd")) {
            if(ISARG(argv[2], "on")) {
                dev.dbg_canfd = true;
            } else if(ISARG(argv[2], "off")) {
                dev.dbg_canfd = false;
            } else {
                return -1;
            }
        } else {
            return -1;
        }
    } else {
        return -1;
    }
    return 0;
}

static int args_ena(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    int ena = strtol(argv[1], nullptr, 0);
    if(ena != 0 && ena != 1) {
        return -1;
    }
    if(ena) {
        dev.ena(XBot::ENABLE);
    } else {
        dev.ena(XBot::DISABLE);
    }
    
    return 0;
}

static QShell::ArgsTable table[] = {
    {"dbg", args_dbg, "[<canfd> <on/off>] - enable/disable dbg info output"},
    {"ena", args_ena, "[<0/1>] - enable/disable xbot joints"},
    
};

int cmd_xbot_hdl(int argc, char **argv)
{
    if(argc == 1) {
        return 0;
    }
    ARGS_TABLE_EXEC(cli, argc, argv, table);
}

EXPORT_CMD("xbot", cmd_xbot_hdl, "xbot cmds");
